using System;

namespace MLRobotic.Asserv.Utils
{
    public class Pid
    {
        private double _error;
        private double _derivativeError;
        private double _integralError;
        private double _lastValue = 0;

        public double Kp { get; set; }
        public double Ki { get; set; }
        public double Kd { get; set; }

        public bool DerivativeOutput { get; set; }

        public double OutputClipping { get; set; }
        public double IntegralClipping { get; set; }
        public double DerivativeClipping { get; set; }

        public bool IntegralOverflow { get; private set; }

        public double Error { get { return _error; } }
        public double DerivativeError { get { return _derivativeError; } }
        public double IntegralError { get { return _integralError; } }

        public Pid() : this (0, 0, 0) {}

        public Pid(double kp, double ki, double kd) : this(kp, ki, kd, false) { }

        public Pid(double kp, double ki, double kd, bool derivativeOutput = false)
        {
            Kp = kp;
            Ki = ki;
            Kd = kd;
            DerivativeOutput = derivativeOutput;
        }

        public double Compute(double setpoint, double measure)
        {
            // Proportionnal
            _error = setpoint - measure;

            // Integral
            _integralError += _error;

            // Derivative
            _derivativeError = DerivativeOutput ? measure - _lastValue : _error - _lastValue;
            _lastValue = DerivativeOutput ? measure : _error;

            // Clippings
            IntegralOverflow = Clip(ref _integralError, IntegralClipping);
            Clip(ref _derivativeError, DerivativeClipping);

            // Compute
            double result = Kp * _error + Ki * _integralError + Kd * _derivativeError;

            Clip(ref result, OutputClipping);

            return result;
        }

        public void Reset()
        {
            _integralError = 0;
            _lastValue = 0;
        }

        private bool Clip(ref double value, double limit)
        {
            if (limit > 0 && value > limit)
            {
                value = limit;
                return true;
            }

            if (limit > 0 && value < -limit)
            {
                value = -limit;
                return true;
            }

            return false;
        }
    }
}
